#include "cv_common.hpp"

namespace {

const int kMAX = 680;
const int kLENGTH = 640;
constexpr int kOFFSET = (kMAX - kLENGTH) / 2;
constexpr int kCENTER = kMAX / 2;
constexpr int kHALF = kLENGTH / 2;

}  // namespace

namespace component {

void Draw(const cv::Mat& output, const cv::Rect2f& rect,
          const cv::Scalar& color, bool add_lable, cv::MarkerTypes type) {
  cv::rectangle(output, rect, color, 2);

  auto center = rect.tl() + cv::Point2f(rect.size()) / 2;
  cv::drawMarker(output, center, color, type);

  if (add_lable) {
    cv::putText(output, cv::format("%.2f, %.2f", center.x, center.y), center,
                painting::kCV_FONT, 1.0, color);
  }
}

void DrawInfo(const cv::Mat& output, const std::string& info, int level,
              const cv::Scalar& color) {
  int v_pos = 0;
  while (level < 0) level += 20;
  v_pos += 24 * level;
  cv::putText(output, info, cv::Point(0, v_pos), painting::kCV_FONT, 1.0,
              color);
}

cv::Rect2i Area(const cv::Rect2i& ri, const cv::Rect2i& rj) {
  auto center_i = AutoCenter(ri);
  auto center_j = AutoCenter(rj);
  if (center_i.x > center_j.x) std::swap(center_i, center_j);
  auto height = std::min(ri.height, rj.height);
  cv::Rect2i rect(cv::Point2i(center_i.x, center_i.y + height),
                  cv::Point2i(center_j.x, center_j.y + 3 * height));
  return rect;
}

cv::Point2i Convert(const LidarPoint& pt, double offset) {
  auto dist = pt.dist_ / 1000 * offset;
  auto theta = (pt.angle_ - 180) / 180 * M_PI;
  return cv::Point2i(static_cast<int>(std::round(offset + dist * sin(theta))),
                     static_cast<int>(std::round(offset + dist * cos(theta))));
}

cv::Mat ViualizeBackground() {
  cv::Mat background = cv::Mat(kMAX, kMAX, CV_8UC3, painting::kBLACK);

  cv::line(background, {kCENTER, kOFFSET}, {kCENTER, kLENGTH + kOFFSET},
           painting::kYELLOW, 1);
  cv::line(background, {kOFFSET, kCENTER}, {kLENGTH + kOFFSET, kCENTER},
           painting::kYELLOW, 1);
  cv::circle(background, {kCENTER, kCENTER}, kLENGTH / 4, painting::kRED, 1,
             cv::LineTypes::LINE_AA);
  cv::circle(background, {kCENTER, kCENTER}, kHALF, painting::kRED, 1,
             cv::LineTypes::LINE_AA);

  for (int i = -10; i <= 10; i++) {
    if (i != 0) {
      // X axis marker
      cv::line(background, {kCENTER, kCENTER + i * 32},
               {kCENTER + 5, kCENTER + i * 32}, painting::kYELLOW, 1);
      // Y axis marker
      cv::line(background, {kCENTER + i * 32, kCENTER},
               {kCENTER + i * 32, kCENTER - 5}, painting::kYELLOW, 1);
      if (i % 2 == 0) {
        // X axis number
        cv::putText(background, cv::format("%-2d", abs(i)),
                    {kCENTER + 7, kCENTER + i * 32 + 4}, painting::kCV_FONT,
                    0.4, painting::kYELLOW, 1);
        // Y axis number
        cv::putText(background, cv::format("%-2d", abs(i)),
                    {kCENTER + i * 32 - 4, kCENTER - 10}, painting::kCV_FONT,
                    0.4, painting::kYELLOW, 1);
      }
    }
  }

  cv::arrowedLine(background, {20, 40}, {20, 20}, painting::kRED, 2, 8, 0, 0.3);
  cv::putText(background, std::string("Front"), {30, 40}, painting::kCV_FONT, 1,
              painting::kRED, 2);

  return background;
}

bool VisualizeLidarNode(cv::Mat& background,
                        const std::vector<component::LidarPoint>& points) {
  auto draw = [&](component::LidarPoint point) {
    if (point.angle_ * point.dist_ == 0) return;
    auto dist = point.dist_ / 1000 * kHALF;
    auto theta = (point.angle_ - 180) / 180 * M_PI;
    auto dst_pt =
        cv::Point2d(kCENTER + dist * sin(theta), kCENTER + dist * cos(theta));
    cv::circle(background, dst_pt, 1, painting::kRED, -1);
  };

  std::for_each(points.begin(), points.end(), draw);

  return true;
}

}  // namespace component